
#define Mode_USR 0x10
#define Mode_FIQ 0x11
#define Mode_IRQ 0x12
#define Mode_SVC 0x13
#define Mode_MON 0x16
#define Mode_ABT 0x17
#define Mode_HYP 0x1a
#define Mode_UND 0x1b
#define Mode_SYS 0x1f
#define I_Bit    0x80
#define F_Bit    0x40

.section .text.start, "ax"
.global asm_start
asm_start:
    # bl  build_initial_pgtable

    //enter svc mode and set the stack pointer
    // word gs_svc_stack[NR_CORE][1024]
    // sp_core = gs_svc_stack + 1024*(core+1)*4
    //         = gs_svc_stack + (core+1)<<12
    msr cpsr_c, #Mode_SVC|I_Bit|F_Bit //disable irq and fiq
    //get processor coreid
    mrc p15, 0, r0, c0, c0, 5
    and r0, r0, #3 
    mov r11, r0
    add r0, r0, #1

    ldr sp, =gABTStack
    add sp, sp, r0, lsl #12
    msr cpsr_c, #Mode_ABT|I_Bit|F_Bit //disable irq and fiq

    ldr sp, =gUNDStack
    add sp, sp, r0, lsl #12
    msr cpsr_c, #Mode_UND|I_Bit|F_Bit //disable irq and fiq

    ldr sp, =gIRQStack
    add sp, sp, r0, lsl #12
    msr cpsr_c, #Mode_IRQ|I_Bit|F_Bit //disable irq and fiq

    ldr sp, =gSVCStack
    add sp, sp, r0, lsl #12
    msr cpsr_c, #Mode_SVC|I_Bit|F_Bit //disable irq and fiq

    bl  enable_l1_cache
    bl  enable_scu
    mov r0, r11
    bl  join_smp
    bl  enable_l1_cache
    cmp r11, #0
    bne ap_core_init

bp_core_init:
    mov  r4, #0 
    ldr r0, =__bss_begin__
    ldr r1, =__bss_end__
clean_bss:
    str r4, [r0]
    add r0, r0, #4
    cmp r0, r1
    bne clean_bss

    adr r0, asm_start
    b  startup_astral

ap_core_init:
    b  startup_apcore

//# define CONFIG_MMU
#ifdef CONFIG_MMU
    // build 1st section
build_initial_pgtable:
    //build page table  1-level
    adr r0, asm_start       //page table at LOADADDR
    # sub r0, r0, #0x10000
    mov r4, r0              //r4 always holds the pagetable[0] address
    mov r1, #4096           // 4096 entries
    mov r2, #0
    clean_pt_all:
    str r2, [r0],#0x04         // pagetable[r0] = 0, r0++
    subs r1, r1, #1            // r1-- 
    bgt clean_pt_all           // loop r1=4096 times
    //clean done at here


    # //ok, let us build a identity map(1)
    # //for pheriphal (vexpress)
    # ldr r0, =HEX(1000,0000)
    # ldr r1, =HEX(1000,0000)
    # mov r2, #2                // 1M identify for pheriphal
    # mov r3, #0x412            // writeable readable executable
    # bl set_page_withoffset
    //ok, let us build a identity map at 0x6000_0000
    ldr r0, =LOADADDR
    ldr r1, =LOADADDR
    mov r2, #2                // 2M identify for pheriphal
    mov r3, #0x412            // writeable readable executable
    bl set_page_withoffset
    //now, map pa(0x6010_0000) to va(0x8000_0000)
    ldr r0, =LOADADDR
    ldr r1, =KDM_START
    mov r2, #20               // 20M 
    mov r3, #0x412            // writeable readable executable
    bl set_page_withoffset

    # //set vector region
    # ldr r0, =HEX(6100,0000)
    # ldr r1, =HEX(ffff,0000)
    # mov r2, #1               // 20M 
    # mov r3, #0x412            // writeable readable executable
    # bl set_page_withoffset

    //ok, page has been done

    //=============================================================================
    // start enable MMU
    // set TTB pointing at pagetable[0] (physical)
    mov r0, r4
    mcr p15, 0, r0, c2, c0, 0
    mcr p15, 0, r0, c8, c7, 0  // flush TLB

    // set domain0: 01=client(check permission) 11=master(no check)
    mov r0, #0x1               // 01 for CLIENT
    mcr p15, 0, r0, c3, c0, 0  // write 0x11=MASTER to domain REG c3

    // enable MMU 
    mrc p15, 0, r0, c1, c0, 0   // read control REG c1 into r0
    // c1 bit-13 = 1 => remap Vectors to 0xFFFF0000-0xFFFF001C
    mov r0, #0
    orr r0, r0, #0x00002000  // NOTE: set C1 bit_13 (remap vector to 0xFFFF0000)
    orr r0, r0, #0x00000001      // set bit0 of r0 to 1
    mcr p15, 0, r0, c1, c0, 0    // write to control REG c1 ==> MMU on
    nop
    nop
    nop
    mrc p15, 0, r2, c2, c0, 0   // read TLB base reg c2 into r2
    nop
    nop
    dsb
    //==============================================================================


#endif //CONFIG_MMU

.end
